#include "MyRobot.h"


/**
 * This file includes the constructor for the RobotDemo class,
 * and also the Test() method used for testing code through
 * the driver station.
 */

//Anything on the robot physically that the program needs to
//communicate with MUST be initialized here before using!
//These MUST be in the same order as they are defined in MyRobot.h!
RobotDemo::RobotDemo() :
	// these must be initialized as they are declared in MyRobot.h
	motor5(5),	//CHANGE THESE NUMBERS (and the rest) based on how they are connected to the digital sidecar
	motor1(1),	//here
	motor4(4),	//here
	motor3(3),	//here
	
	
	one(0),
	myRobot(one, one, one, one),
	
	motor2(2),	//here
	
	leftStick(1),
	rightStick(2),
	xbox(3),
	//display(FOUR_ZONES),	//FOUR_ZONES, ONE_TWO_LINE_ZONE, TWO_ONE_LINE_ZONES
	liftUp(1),	//here
	liftDown(2), //here
	armDown(3), //here
	//encoderLeft(3),	//here
	//encoderRight(4),	//here
	//pressureTransducer(1),	//analog input, might have to give module position
	ledSpike(1, Relay::kForwardOnly),
	//compRelay(8),
	/*
	compressor(4,2), //here (pressure switch channel, relay channel
	launchers(2,1),
	latch(2,3),
	shifters(2,5),
	retractLaunchers(2,2),
	retractLatch(2,4),
	retractShifters(2,6),
	encoder(5, 6, false),
	*/
	enableAuton(7),
	switchAuton(8)
	
	//5 & 6 are shifters 
	//3 & 4 are latch
	//1 & 2 are launch

	//DEBUG PITFALLS:
	//commas between all initializations?
	//extra comma at the end?
	{
			myRobot.SetExpiration(0.1);
	}
	
/**
 * Runs during test mode
 */
void RobotDemo::Test()
{
	UpdateSmartDashboard(true); //init the LiveWindow smart dashboard
	
	LiveWindow::GetInstance()->SetEnabled(true);
	
	while (IsTest() && IsEnabled())
	{
		//Insert test code here
		LiveWindow::GetInstance()->Run();
	}
	
	LiveWindow::GetInstance()->SetEnabled(false);
}
START_ROBOT_CLASS(RobotDemo);


